#ifndef _QUATERNION_HPP_
#define _QUATERNION_HPP_

#include <math.h>

#include "Vector3.hpp"

#include "Matrix4.hpp"

class Quaternion {
public:

	float w;

	float x;
	
	float y;
	
	float z;
	
public:

	float* getPtr() {
		return (float*)this;
	}

	const float* getPtr() const {
		return (float*)this;
	}
	
	float& operator [] (unsigned int index) {
		return ((float*)this)[index];
	}

	float operator [] (unsigned int index) const {
		return ((float*)this)[index];
	}

	bool operator == (const Quaternion& q) const {
		return w == q.w && x == q.x && y == q.y && z == q.z;
	}
	
	bool operator != (const Quaternion& q) const {
		return w != q.w || x != q.x || y != q.y || z != q.z;
	}
	
	Quaternion operator + (const Quaternion& q) const {
		return Quaternion(w + q.w, x + q.x, y + q.y, z + q.z);
	}

	Quaternion operator - (const Quaternion& q) const {
		return Quaternion(w - q.w, x - q.x, y - q.y, z - q.z);
	}
	
	Quaternion operator * (const Quaternion& q) const {
		return Quaternion(
			w * q.w - x * q.x - y * q.y - z * q.z,
			w * q.x + x * q.w + y * q.z - z * q.y,
			w * q.y + y * q.w + z * q.x - x * q.z,
			w * q.z + z * q.w + x * q.y - y * q.x
		);
	}
	
	void operator += (const Quaternion& q) {
		w += q.w;
		x += q.x;
		y += q.y;
		z += q.z;
	}
	
	void operator -= (const Quaternion& q) {
		w -= q.w;
		x -= q.x;
		y -= q.y;
		z -= q.z;
	}
	
	void operator *= (const Quaternion& q) {
		*this = *this * q;
	}
	
	Quaternion operator + (float s) const {
		return Quaternion(w + s, x + s, y + s, z + s);
	}
	
	Quaternion operator - (float s) const {
		return Quaternion(w + s, x + s, y + s, z + s);
	}
	
	Quaternion operator * (float s) const {
		return Quaternion(w * s, x * s, y * s, z * s);
	}
	
	Quaternion operator / (float s) const {
		return Quaternion(w / s, x / s, y / s, z / s);
	}
	
	void operator += (float s) {
		w += s;
		x += s;
		y += s;
		z += s;
	}
	
	void operator -= (float s) {
		w -= s;
		x -= s;
		y -= s;
		z -= s;
	}
	
	void operator *= (float s) {
		w *= s;
		x *= s;
		y *= s;
		z *= s;
	}
	
	void operator /= (float s) {
		w /= s;
		x /= s;
		y /= s;
		z /= s;
	}
	
public:

	/*
		Скалярное произведение (dot product)
	*/

	static float dot(const Quaternion& q1, const Quaternion q2) {
		return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
	}

	/*
		Векторное произведение (cross product)
	*/

	static Quaternion cross(const Quaternion& q1, const Quaternion& q2) {
		return Quaternion(
			q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
			q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
			q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z,
			q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x
		);
	}

	/*
		Длина
	*/
	
	float length() const {
		return sqrtf(w * w + x * x + y * y + z * z);
	}

	/*
		Длина
	*/
	
	static float length(const Quaternion& q) {
		return q.length();
	}
	
	/*
		Нормализация
	*/
	
	static Quaternion normalize(const Quaternion& q) {
		float len = length(q);
		if (len < 0.0f) {
			return Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
		}
		return q * (1.0f / len);
	}
	
	/*
		Инверсия	
	*/
	
	static Quaternion inverse(const Quaternion& q) {
		float dotQQ = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
		return Quaternion(q.w / dotQQ, -q.x / dotQQ, -q.y / dotQQ, -q.z / dotQQ);
	}
	
	/*
		Сферическая линейная интерполяция
	*/
	
	static Quaternion slerp(const Quaternion& q1, const Quaternion& q2, float a) {
		Quaternion q3;
		float cosTheta = dot(q1, q2);
		if (cosTheta < 0.0f) {
			cosTheta = -cosTheta;
			q3.w = -q2.w;
			q3.x = -q2.x;
			q3.y = -q2.y;
			q3.z = -q2.z;
		} else {
			q3 = q2;
		}
		if (cosTheta > 0.9999999f) {
			return Quaternion(
				(q3.w - q1.w) * a + q1.w,
				(q3.x - q1.x) * a + q1.x,
				(q3.y - q1.y) * a + q1.y,
				(q3.z - q1.z) * a + q1.z
			);
		} else {
			float angle = acos(cosTheta);
			return (q1 * sin((1.0f - a) * angle) + q3 * sin(a * angle)) / sin(angle);
		}
	}
	
	/*
		Конвертирует кватернион в матрицу поворота
	*/
	
	Matrix4 toMatrix4() const {
		float xx = x * x;
		float yy = y * y;
		float zz = z * z;
		float xz = x * z;
		float xy = x * y;
		float yz = y * z;
		float wx = w * x;
		float wy = w * y;
		float wz = w * z;
		return Matrix4 (
			1.0f - 2.0f * (yy + zz),
			2.0f * (xy + wz),
			2.0f * (xz - wy),
			0.0f,
			2.0f * (xy - wz),
			1.0f - 2.0f * (xx + zz),
			2.0f * (yz + wx),
			0.0f,
			2.0f * (xz + wy),
			2.0f * (yz - wx),
			1.0f - 2.0f * (xx + yy),
			0.0f,
			0.0f,
			0.0f,
			0.0f,
			1.0f
		);
	}
	
	/*
		Возвращает кватернион повернутый на углы Эйлера
	*/
	
	static Quaternion EulerToQuaternion(float pitch, float yaw, float roll) {
		float cPitch = cos(pitch * 0.5f);
		float cYaw = cos(yaw * 0.5f);
		float cRoll = cos(roll * 0.5f);
		float sPitch = sin(pitch * 0.5f);
		float sYaw = sin(yaw * 0.5f);
		float sRoll = sin(roll * 0.5f);
		return Quaternion(
			cPitch * cYaw * cRoll + sPitch * sYaw * sRoll,
			sPitch * cYaw * cRoll - cPitch * sYaw * sRoll,
			cPitch * sYaw * cRoll + sPitch * cYaw * sRoll,
			cPitch * cYaw * sRoll - sPitch * sYaw * cRoll
		);
	}
	
	/*
		Возвращает кватернион повернутый на углы Эйлера
	*/

	static Quaternion EulerToQuaternion(const Vector3& eulerAngles) {
		float cPitch = cos(eulerAngles.x * 0.5f);
		float cYaw = cos(eulerAngles.y * 0.5f);
		float cRoll = cos(eulerAngles.z * 0.5f);
		float sPitch = sin(eulerAngles.x * 0.5f);
		float sYaw = sin(eulerAngles.y * 0.5f);
		float sRoll = sin(eulerAngles.z * 0.5f);
		return Quaternion(
			cPitch * cYaw * cRoll + sPitch * sYaw * sRoll,
			sPitch * cYaw * cRoll - cPitch * sYaw * sRoll,
			cPitch * sYaw * cRoll + sPitch * cYaw * sRoll,
			cPitch * cYaw * sRoll - sPitch * sYaw * cRoll
		);
	}
	
	/*
		Поворот вокруг вектора
	*/
	
	static Quaternion rotate(const Quaternion& q, float angle, const Vector3& vector) {
		Vector3 v = Vector3::normalize(vector);
		angle *= 0.5f;
		float s = sin(angle);
		return q * Quaternion(cos(angle), v.x * s, v.y * s, v.z * s);
	}
	
	/*
		Поворот вокруг вектора
	*/
	
	static Quaternion rotate(const Quaternion& q, float angle, float x, float y, float z) {
		Vector3 v = Vector3::normalize(Vector3(x, y, z));
		angle *= 0.5f;
		float s = sin(angle);
		return q * Quaternion(cos(angle), v.x * s, v.y * s, v.z * s);
	}

	/*
		Поворот вокруг вектора
	*/

	void rotate(float angle, const Vector3& vector) {
		Vector3 v = Vector3::normalize(vector);
		float s = sin(angle * 0.5f);
		*this *= Quaternion(cos(angle * 0.5f), v.x * s, v.y * s, v.z * s);
	}
	
	/*
		Поворот вокруг вектора
	*/
	
	void rotate(Quaternion q, float angle, float x, float y, float z) {
		Vector3 v = Vector3::normalize(Vector3(x, y, z));
		float s = sin(angle * 0.5f);
		*this *= Quaternion(cos(angle * 0.5f), v.x * s, v.y * s, v.z * s);
	}
	
	/*
		Конвертирует кватернион в углы Эйлера
	*/
	
	Vector3 toEuler() const {
		return Vector3(getPitch(), getYaw(), getRoll());
	}
	
	/*
		Вернет pitch угол Эйлера
	*/
	
	float getPitch() const {
		return atan2(2.0f * (y * z + w * x), w * w - x * x - y * y + z * z);		
	}
	
	/*
		Вернет pitch угол Эйлера
	*/
	
	float getYaw() const {
		return asin(-2.0f * (x * z - w * y));
	}
	
	/*
		Вернет pitch угол Эйлера
	*/
	
	float getRoll() const {
		return atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z);		
	}
	
public:

	Quaternion(float pitch, float yaw, float roll) {
		float cPitch = cos(pitch * 0.5f);
		float cYaw = cos(yaw * 0.5f);
		float cRoll = cos(roll * 0.5f);
		float sPitch = sin(pitch * 0.5f);
		float sYaw = sin(yaw * 0.5f);
		float sRoll = sin(roll * 0.5f);
		w = cPitch * cYaw * cRoll + sPitch * sYaw * sRoll;
		x = sPitch * cYaw * cRoll - cPitch * sYaw * sRoll;
		y = cPitch * sYaw * cRoll + sPitch * cYaw * sRoll;
		z = cPitch * cYaw * sRoll - sPitch * sYaw * cRoll;
	}
	
	Quaternion(const Vector3& eulerAngles) {
		float cPitch = cos(eulerAngles.x * 0.5f);
		float cYaw = cos(eulerAngles.y * 0.5f);
		float cRoll = cos(eulerAngles.z * 0.5f);
		float sPitch = sin(eulerAngles.x * 0.5f);
		float sYaw = sin(eulerAngles.y * 0.5f);
		float sRoll = sin(eulerAngles.z * 0.5f);
		w = cPitch * cYaw * cRoll + sPitch * sYaw * sRoll;
		x = sPitch * cYaw * cRoll - cPitch * sYaw * sRoll;
		y = cPitch * sYaw * cRoll + sPitch * cYaw * sRoll;
		z = cPitch * cYaw * sRoll - sPitch * sYaw * cRoll;
	}

	Quaternion(float w, float x = 0.0f, float y = 0.0f, float z = 0.0f) {
		Quaternion::w = w;
		Quaternion::x = x;
		Quaternion::y = y;
		Quaternion::z = z;
	}

	Quaternion() {
	}
	
};

#endif